#ifndef COMPENSATOR_H
#define COMPENSATOR_H

#include "DataType.hpp"
#include "armor.hpp"
#include <opencv2/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>
#include <string>
#include "angles.h"

namespace hnurm
{
    /**
     * @brief 补偿器类
     */
    class Compensator
    {
    private:
        // 默认参数
        float gravity_ = 9.8; /**< 重力加速度 */
        float friction_coeff_ = 0.05; /**< 摩擦系数 */
        float bullet_speed_bias_ = -1; /**< 子弹速度偏置 */
        float imutofrt_y=0;
        float imutofrt_z=0;
        TargetInfo t_info; /**< 目标信息 */
        float fx, fy, fz, fx_center, fy_center; /**< 附加质量 */
        float pout, yout; /**< 输出变量 */
        float tmp_fly_time; /**< 子弹飞行时间 */
        float bullet_speed_; /**< 子弹速度 */
        void PredictPose(ArmorsNum &num); /**< 预测姿态 */
        void CompensatePitch(); /**< 补偿俯仰角 */
        [[nodiscard]] double diff_eq(double v) const;
        double integrate_euler(double v0, double t);
    public:
        float channel_delay_; /**< 通道延迟 */

        /**
         * @brief 构造函数
         * @param cfg_node 配置节点
         */
        explicit Compensator(const cv::FileNode &cfg_node);

        /**
         * @brief 计算最终角度
         * @param target_msg 目标消息
         * @param recv_data 接收数据
         * @param send_data 发送数据
         */
        float CalcFinalAngle(TargetInfo &target_msg, VisionRecvData &recv_data, VisionSendData &send_data,ArmorsNum &num);

        /**
         * @brief 用于比较两个三维点的坐标值
         * @param a 第一个三维点
         * @param b 第二个三维点
         * @return 比较结果
         */
        static bool cmp(const cv::Point3f& a, const cv::Point3f& b);
    };
}// namespace hnurm

#endif// COMPENSATOR_H
